﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using System.Diagnostics;

namespace Oops.Xna.Framework.Physics
{
	/// <summary>
	/// Defines a collision shape in the form of a mesh.
	/// </summary>
	public class CollisionMesh : CollisionShape 
	{
		private CollisionMeshProxy proxy;

		private List<CollisionMeshTriangle> triangles = new List<CollisionMeshTriangle>(32);

		internal Matrix transform = Matrix.Identity;
		internal Matrix inverseTransform = Matrix.Identity;

		/// <summary>
		/// Initializes a new instance of the <see cref="CollisionMesh"/> class.
		/// </summary>
		/// <param name="proxy">The <see cref="CollisionMeshProxy"/> that will be used to retrieve information about the mesh.</param>
		public CollisionMesh(CollisionMeshProxy proxy) : base(CollisionShapeType.Mesh)
		{
			// The proxy parameter must be specified.
			if (proxy == null)
			{
				throw new ArgumentNullException("proxy");
			}

			// Hold onto parameters.
			this.proxy = proxy;
		}

		/// <summary>
		/// Returns the associated <see cref="CollisionMeshProxy"/> of the <see cref="CollisionMesh"/>.
		/// </summary>
		public CollisionMeshProxy Proxy
		{
			get { return this.proxy; }
		}

		#region CollisionShape Members

		internal override void CalculateInternals()
		{
			// Get the inverse of this shape's transform.
#if QUATERNION_ORIENTATION
			this.transform = Matrix.CreateFromQuaternion(this.Orientation) * Matrix.CreateTranslation(this.Position);
			this.inverseTransform = Matrix.Invert(this.transform);
#else
#if REFERENCE
			Matrix.CreateTranslation(ref this.Position, out this.transform);
			Matrix.Multiply(ref this.Orientation, ref this.transform, out this.transform);
			Matrix.Invert(ref this.transform, out this.inverseTransform);
#else
			this.transform = this.Orientation * Matrix.CreateTranslation(this.Position);
			this.inverseTransform = Matrix.Invert(this.transform);
#endif
#endif
			// Call inherited method.
			base.CalculateInternals();
		}

		protected override void CalculateAABB()
		{
			// Transform the proxy's axis-aligned bounding box to world-space.
#if INLINE
#if XBOX
			Vector3 extents = Vector3.Zero;
#else
			Vector3 extents;
#endif
			extents.X = (this.proxy.AABB.Max.X - this.proxy.AABB.Min.X) * 0.5f;
			extents.Y = (this.proxy.AABB.Max.Y - this.proxy.AABB.Min.Y) * 0.5f;
			extents.Z = (this.proxy.AABB.Max.Z - this.proxy.AABB.Min.Z) * 0.5f;
#if XBOX
			Vector3 center = Vector3.Zero;
#else
			Vector3 center;
#endif
			center.X = this.proxy.AABB.Min.X + extents.X;
			center.Y = this.proxy.AABB.Min.Y + extents.Y;
			center.Z = this.proxy.AABB.Min.Z + extents.Z;
#elif REFERENCE
			Vector3 center, extents;
			BoundingBoxExtension.GetCenter(ref this.proxy.AABB, out center);
			BoundingBoxExtension.GetExtents(ref this.proxy.AABB, out extents);
#else
			Vector3 center = this.proxy.AABB.GetCenter();
			Vector3 extents = this.proxy.AABB.GetExtents();
#endif
#if QUATERNION_ORIENTATION
			Matrix orientation = Matrix.CreateFromQuaternion(this.Orientation);

			float x = Math.Abs(orientation.M11 * extents.X) + Math.Abs(orientation.M21 * extents.Y) + Math.Abs(orientation.M31 * extents.Z);
			float y = Math.Abs(orientation.M12 * extents.X) + Math.Abs(orientation.M22 * extents.Y) + Math.Abs(orientation.M32 * extents.Z);
			float z = Math.Abs(orientation.M13 * extents.X) + Math.Abs(orientation.M23 * extents.Y) + Math.Abs(orientation.M33 * extents.Z);
#else
			float x = Math.Abs(this.Orientation.M11 * extents.X) + Math.Abs(this.Orientation.M21 * extents.Y) + Math.Abs(this.Orientation.M31 * extents.Z);
			float y = Math.Abs(this.Orientation.M12 * extents.X) + Math.Abs(this.Orientation.M22 * extents.Y) + Math.Abs(this.Orientation.M32 * extents.Z);
			float z = Math.Abs(this.Orientation.M13 * extents.X) + Math.Abs(this.Orientation.M23 * extents.Y) + Math.Abs(this.Orientation.M33 * extents.Z);
#endif
			// Update the axis-aligned bounding box for this shape.
			this.AABB.Min.X = this.Position.X + center.X - x;
			this.AABB.Max.X = this.Position.X + center.X + x;
			this.AABB.Min.Y = this.Position.Y + center.Y - y;
			this.AABB.Max.Y = this.Position.Y + center.Y + y;
			this.AABB.Min.Z = this.Position.Z + center.Z - z;
			this.AABB.Max.Z = this.Position.Z + center.Z + z;
		}

		public override bool IntersectsWith(ref CollisionRay ray, out Vector3 position, out Vector3 normal, out float t)
		{
			// Initialize return values.
			position = Vector3.Zero;
			normal = Vector3.Zero;
			t = float.MaxValue;

			// Copy and transform ray into mesh-space.
			CollisionRay localRay;
#if INLINE
#if XBOX
			localRay = CollisionRay.Empty;
#endif
			localRay.Position.X = ray.Position.X * this.inverseTransform.M11 + ray.Position.Y * this.inverseTransform.M21 + ray.Position.Z * this.inverseTransform.M31 + this.inverseTransform.M41;
			localRay.Position.Y = ray.Position.X * this.inverseTransform.M12 + ray.Position.Y * this.inverseTransform.M22 + ray.Position.Z * this.inverseTransform.M32 + this.inverseTransform.M42;
			localRay.Position.Z = ray.Position.X * this.inverseTransform.M13 + ray.Position.Y * this.inverseTransform.M23 + ray.Position.Z * this.inverseTransform.M33 + this.inverseTransform.M43;

			localRay.Vector.X = ray.Vector.X * this.inverseTransform.M11 + ray.Vector.Y * this.inverseTransform.M21 + ray.Vector.Z * this.inverseTransform.M31;
			localRay.Vector.Y = ray.Vector.X * this.inverseTransform.M12 + ray.Vector.Y * this.inverseTransform.M22 + ray.Vector.Z * this.inverseTransform.M32;
			localRay.Vector.Z = ray.Vector.X * this.inverseTransform.M13 + ray.Vector.Y * this.inverseTransform.M23 + ray.Vector.Z * this.inverseTransform.M33;
#elif REFERENCE
			Vector3.Transform(ref ray.Position, ref this.inverseTransform, out localRay.Position);
			Vector3.TransformNormal(ref ray.Vector, ref this.inverseTransform, out localRay.Vector);
#else
			localRay.Position = Vector3.Transform(ray.Position, this.inverseTransform);
			localRay.Vector = Vector3.TransformNormal(ray.Vector, this.inverseTransform);
#endif
			// Attempt to get all triangles that intersect with the collision ray.
			this.triangles.Clear();
			if (this.proxy.GetIntersectingTriangles(ref localRay, this.triangles))
			{
				// Working storage.
				Vector3 localPosition;
				float localT;

				// Iterate on the potentially intersecting triangles.
				for (int i = 0; i < this.triangles.Count; i++)
				{
					// Get the current triangle.
					CollisionMeshTriangle triangle = this.triangles[i];

					// Determine intersection with the current triangle.
					if (triangle.IntersectsWith(ref localRay, out localPosition, out localT))
					{
						// Hold onto results.
						position = localPosition;
						normal = triangle.Normal;
						t = localT;

						// Adjust the ray's length and try again.
#if INLINE
						localRay.Vector.X = localRay.Vector.X * t;
						localRay.Vector.Y = localRay.Vector.Y * t;
						localRay.Vector.Z = localRay.Vector.Z * t;
#elif REFERENCE
						Vector3.Multiply(ref localRay.Vector, t, out localRay.Vector);
#else
						localRay.Vector *= t;
#endif
					}
				}

				// Recycle triangles.
				this.ReleaseIntersectingTriangles(this.triangles);
			}

			// An intersection was found. Transform the results into world-space.
			if (t <= 1.0f)
			{
#if INLINE
				float x = position.X * this.transform.M11 + position.Y * this.transform.M21 + position.Z * this.transform.M31 + this.transform.M41;
				float y = position.X * this.transform.M12 + position.Y * this.transform.M22 + position.Z * this.transform.M32 + this.transform.M42;
				float z = position.X * this.transform.M13 + position.Y * this.transform.M23 + position.Z * this.transform.M33 + this.transform.M43;
				position.X = x;
				position.Y = y;
				position.Z = z;

				x = normal.X * this.transform.M11 + normal.Y * this.transform.M21 + normal.Z * this.transform.M31;
				y = normal.X * this.transform.M12 + normal.Y * this.transform.M22 + normal.Z * this.transform.M32;
				z = normal.X * this.transform.M13 + normal.Y * this.transform.M23 + normal.Z * this.transform.M33;
				normal.X = x;
				normal.Y = y;
				normal.Z = z;
#elif REFERENCE
				Vector3.Transform(ref position, ref this.transform, out position);
				Vector3.TransformNormal(ref normal, ref this.transform, out normal);
#else
				position = Vector3.Transform(position, this.transform);
				normal = Vector3.TransformNormal(normal, this.transform);
#endif
				return true;
			}

			// An intersection was not found.
			return false;
		}

		protected internal override float GetVolume()
		{
			return 0.0f;
		}

		public override void CalculateMassProperties(float density, out float mass, out Matrix inertiaTensor)
		{
			mass = 0.0f;
			inertiaTensor = MatrixExtension.Zero;
		}

		#endregion

		/// <summary>
		/// Gets all <see cref="CollisionTriangle"/> instances from the <see cref="CollisionMesh"/> whose bounds
		/// intersect with the specified <see cref="Microsoft.Xna.Framework.BoundingBox"/>.
		/// </summary>
		/// <param name="aabb">The source <see cref="Microsoft.Xna.Framework.BoundingBox"/>.</param>
		/// <param name="result">The collection to store all colliding triangles from the <see cref="CollisionMesh"/>.</param>
		/// <returns><b>true</b> if the specified <paramref name="aabb"/> collides with any triangle from the <see cref="CollisionMesh"/>.</returns>
		internal bool GetIntersectingTriangles(ref BoundingBox aabb, List<CollisionMeshTriangle> result)
		{
			// Transform the axis-aligned bounding box to object space.
#if XBOX
			BoundingBox localAABB = new BoundingBox();
#else
			BoundingBox localAABB;
#endif
#if QUATERNION_ORIENTATION
			Vector3 center = aabb.GetCenter();
			Vector3 extents = aabb.GetExtents();

			float x = center.X - this.Position.X;
			float y = center.Y - this.Position.Y;
			float z = center.Z - this.Position.Z;

			Matrix orientation = Matrix.CreateFromQuaternion(this.Orientation);

			center.X = x * orientation.M11 + y * orientation.M12 + z * orientation.M13;
			center.Y = x * orientation.M21 + y * orientation.M22 + z * orientation.M23;
			center.Z = x * orientation.M31 + y * orientation.M32 + z * orientation.M33;

			x = Math.Abs(orientation.M11 * extents.X) + Math.Abs(orientation.M12 * extents.Y) + Math.Abs(orientation.M13 * extents.Z);
			y = Math.Abs(orientation.M21 * extents.X) + Math.Abs(orientation.M22 * extents.Y) + Math.Abs(orientation.M23 * extents.Z);
			z = Math.Abs(orientation.M31 * extents.X) + Math.Abs(orientation.M32 * extents.Y) + Math.Abs(orientation.M33 * extents.Z);
#else
#if INLINE
#if XBOX
			Vector3 extents = Vector3.Zero;
#else
			Vector3 extents;
#endif
			extents.X = (aabb.Max.X - aabb.Min.X) * 0.5f;
			extents.Y = (aabb.Max.Y - aabb.Min.Y) * 0.5f;
			extents.Z = (aabb.Max.Z - aabb.Min.Z) * 0.5f;
#if XBOX
			Vector3 center = Vector3.Zero;
#else
			Vector3 center;
#endif
			center.X = aabb.Min.X + extents.X;
			center.Y = aabb.Min.Y + extents.Y;
			center.Z = aabb.Min.Z + extents.Z;
#elif REFERENCE
			Vector3 center, extents;
			BoundingBoxExtension.GetCenter(ref aabb, out center);
			BoundingBoxExtension.GetExtents(ref aabb, out extents);
#else
			Vector3 center = aabb.GetCenter();
			Vector3 extents = aabb.GetExtents();
#endif
			float x = center.X - this.Position.X;
			float y = center.Y - this.Position.Y;
			float z = center.Z - this.Position.Z;

			center.X = x * this.Orientation.M11 + y * this.Orientation.M12 + z * this.Orientation.M13;
			center.Y = x * this.Orientation.M21 + y * this.Orientation.M22 + z * this.Orientation.M23;
			center.Z = x * this.Orientation.M31 + y * this.Orientation.M32 + z * this.Orientation.M33;

			x = Math.Abs(this.Orientation.M11 * extents.X) + Math.Abs(this.Orientation.M12 * extents.Y) + Math.Abs(this.Orientation.M13 * extents.Z);
			y = Math.Abs(this.Orientation.M21 * extents.X) + Math.Abs(this.Orientation.M22 * extents.Y) + Math.Abs(this.Orientation.M23 * extents.Z);
			z = Math.Abs(this.Orientation.M31 * extents.X) + Math.Abs(this.Orientation.M32 * extents.Y) + Math.Abs(this.Orientation.M33 * extents.Z);
#endif
			// Update the axis-aligned bounding box for this shape.
			localAABB.Min.X = center.X - x;
			localAABB.Max.X = center.X + x;
			localAABB.Min.Y = center.Y - y;
			localAABB.Max.Y = center.Y + y;
			localAABB.Min.Z = center.Z - z;
			localAABB.Max.Z = center.Z + z;

			// Use the proxy to determine the intersecting triangles.
			return this.proxy.GetIntersectingTriangles(ref localAABB, result);
		}

		/// <summary>
		/// Release use of the <see cref="CollisionMeshTriangle"/> instances returned from a call to <see cref="CollisionMeshProxy.GetIntersectingTriangles"/>.
		/// </summary>
		/// <param name="triangles">The collection of <see cref="CollisionMeshTriangle"/> instances to release.</param>
		internal void ReleaseIntersectingTriangles(IList<CollisionMeshTriangle> triangles)
		{
			// Delegate to proxy.
			this.proxy.ReleaseIntersectingTriangles(triangles);
		}
	}
}
